 /** 
 * @file pid.c
 * @brief PID控制参数配置与计算函数实现
 *        包含底盘、云台、发射机构等模块的PID参数结构体定义，以及PID计算核心函数
 * @author WeiFan Qiu (qiuweifan0523@faxmail.com)
 * @version 1.0
 * @date 2025-08-08
 * 
 * @copyright Copyright (c) 2025  邱伟凡
 * 
 * @par 修改日志:
 *  2025-08-08  初始版本创建   
 * 
 * @bug 待解决问题:
 * 
 */
#include "QWF_Custom_Ctrl.h"
#include "pid.h"
#include "QWF_Math.h"
#include "QWF_Include.h"
#include "QWF_Cmd.h"
/**************************底盘PID参数***************************************/
#if Circuit_Boards!=Head_Circuit_Boards
#include "chassis.h"
/**************************底盘***************************************/
static __align(1) chassis_pid_t Chassis_PID={
/**********************特殊模式参数（跟随/飞坡/能量缓冲）*****************************************/
					.special={						
.follow={//底盘跟随-底盘偏航角PID参数
	.mode={.out_put=1,.ffc=1,.i_trapezoid=0,.i_change=0},
	.KP=3.5,.KI=0,.KD=0,.KF=0,//20
	.max=8000,.min=0,
	.i_max=10000,
	.f_max=2300,
	.dead_max=5,.dead_min=0	
},
.fly={//飞坡模式-底盘偏航角PID参数
	.mode={.out_put=1,.ffc=1,.i_trapezoid=0,.i_change=0},
	.KP=1.5,.KI=0,.KD=0,.KF=0,
	.max=angle_pid_max,.min=0,
	.i_max=pid_i_max,
	.dead_max=0,.dead_min=0	
},
.power={//缓冲能量模式PID参数
	.mode={.out_put=1,.ffc=0,.i_trapezoid=0,.i_change=0},
	.KP=-10,.KI=0,.KD=0,.KF=0,
	.max=60,.min=0,
	.i_max=pid_i_max,
	.dead_max=0,.dead_min=0	
}},
#if Chassis_Wheel_Type==Helf_Helm_Wheel
/**********************前轮******************************************/
					.f={
	.wheel={//轮
		.v={//速度
			.mode={.out_put=1,.ffc=0,.i_trapezoid=0,.i_change=0},	
			.KP=20,.KI=0,.KD=0,.KF=0,
			.max=DJI_c620_output_max,.min=0,
			.i_max=pid_i_max*100,
			.dead_max=0,.dead_min=0	
		}
	},
	.helm={//舵
		.w={//角速度
			.mode={.out_put=1,.ffc=0,.i_trapezoid=0,.i_change=1},	
			.KP=25,.KI=0.05,.KD=0,.KF=0,
			.i_change_a=0.1*pid_i_max,.i_change_b=0.8*pid_i_max,
			#if f_helm_type==DJI_gm6020_new_i	
			.max=DJI_gm6020_new_i_output_max,.min=0,
			#elif	f_helm_type==DJI_gm6020_new_v	
			.max=DJI_gm6020_new_v_output_max,.min=0,
			#elif 	f_helm_type==DJI_gm6020_old
			.max=DJI_gm6020_new_i_output_max,.min=0,	
			#endif
			.i_max=5000,
			.dead_max=0,.dead_min=0	
		},
		.angle={//角度
			.mode={.out_put=1,.ffc=0,.i_trapezoid=0,.i_change=0},	
			.KP=1,.KI=0,.KD=0,.KF=100,
			.max=angle_pid_max,.min=0,
			.i_max=pid_i_max,
			.f_max=3000,
			.dead_max=0.1,.dead_min=-0.1	
		}
	}
},
/***************************后轮*************************************/
							.b={
	.wheel={//轮
	.v={//速度
		.mode={.out_put=1,.ffc=0,.i_trapezoid=0,.i_change=0},	
		.KP=20,.KI=0,.KD=0,.KF=0,
		.max=DJI_c620_output_max,.min=0,
		.i_max=pid_i_max*100,
		.dead_max=0,.dead_min=0	
	}},
	.helm={//舵
		.w={//角速度
			.mode={.out_put=1,.ffc=0,.i_trapezoid=0,.i_change=1},	
			.KP=25,.KI=0.05,.KD=0,.KF=0,
			.i_change_a=0.1*pid_i_max,.i_change_b=0.8*pid_i_max,
			#if b_helm_type==DJI_gm6020_new_i		
			.max=DJI_gm6020_new_i_output_max,.min=0,
			#elif	b_helm_type==DJI_gm6020_new_v	
			.max=DJI_gm6020_new_v_output_max,.min=0,
			#elif 	b_helm_type==DJI_gm6020_old
			.max=DJI_gm6020_new_i_output_max,.min=0,		
			#endif	
			.i_max=5000,
			.dead_max=0,.dead_min=0	
		},
		.angle={//角度
			.mode={.out_put=1,.ffc=0,.i_trapezoid=0,.i_change=0},	
			.KP=1,.KI=0,.KD=0,.KF=100,
			.max=angle_pid_max,.min=0,
			.i_max=pid_i_max,
			.f_max=3000,
			.dead_max=0.1,.dead_min=-0.1
		}
	}
},
/****************************左轮************************************/
							.l={
	.wheel.v={//速度
		.mode={.out_put=1,.ffc=0,.i_trapezoid=0,.i_change=0},	
		.KP=20,.KI=0,.KD=0,.KF=0,
		.max=DJI_c620_output_max,.min=0,
		.i_max=pid_i_max*100,
		.dead_max=0,.dead_min=0	
	}
},				
/******************************右轮*********************************/	
							.r={
	.wheel.v={//速度
		.mode={.out_put=1,.ffc=0,.i_trapezoid=0,.i_change=0},	
		.KP=20,.KI=0,.KD=0,.KF=0,
		.max=DJI_c620_output_max,.min=0,
		.i_max=pid_i_max*100,
		.dead_max=0,.dead_min=0	
	}
},
/*****************************************************************/
#elif Chassis_Wheel_Type==Omni_Wheel||Chassis_Wheel_Type==Mecanum_Wheel
/****************************左前轮************************************/
							.lf={
	.v={//速度
		.mode={.out_put=1,.ffc=0,.i_trapezoid=0,.i_change=0},	
		.KP=20,.KI=0,.KD=0,.KF=0,
		.max=DJI_c620_output_max,.min=0,
		.i_max=pid_i_max*100,
		.dead_max=0,.dead_min=0	
	}
},				
/******************************左后轮*********************************/	
							.lb={
	.v={//速度
		.mode={.out_put=1,.ffc=0,.i_trapezoid=0,.i_change=0},	
		.KP=20,.KI=0,.KD=0,.KF=0,
		.max=DJI_c620_output_max,.min=0,
		.i_max=pid_i_max*100,
		.dead_max=0,.dead_min=0	
	}
},
/****************************右前轮************************************/
							.rf={
	.v={//速度
		.mode={.out_put=1,.ffc=0,.i_trapezoid=0,.i_change=0},	
		.KP=20,.KI=0,.KD=0,.KF=0,
		.max=DJI_c620_output_max,.min=0,
		.i_max=pid_i_max*100,
		.dead_max=0,.dead_min=0	
	}
},				
/******************************右后轮*********************************/	
							.rb={
	.v={//速度
		.mode={.out_put=1,.ffc=0,.i_trapezoid=0,.i_change=0},	
		.KP=20,.KI=0,.KD=0,.KF=0,
		.max=DJI_c620_output_max,.min=0,
		.i_max=pid_i_max*100,
		.dead_max=0,.dead_min=0	
	}
},
#elif Chassis_Wheel_Type==Leg_Wheel
#endif
};
chassis_pid_t* chassis_pid=&Chassis_PID;//底盘的pid
#endif
#if Circuit_Boards!=Chassis_Circuit_Boards
#include "head.h"
/**********************************************头****************************************************/	
								static __align(1)  head_pid_t Head_PID={
/******************************************云台*******************************************************/	
										.gimbal={
/*******************************************pitch*******************************************************/	
										.pitch={
			.w={//角速度
				.mode={.out_put=1,.ffc=0,.i_trapezoid=0,.i_change=0},	
				.i_change_a=0.2*2000,.i_change_b=0.6*2000,
				.KP=-346,.KI=-1.2,.KD=240,.KF=0,
				.max=DJI_gm6020_new_i_output_max,.min=0,
				.i_max=6000,
				.f_max=0,
				.dead_max=0,.dead_min=0	},
			.angle={//角度
				.mode={.out_put=1,.ffc=0,.i_trapezoid=0,.i_change=0},	
				.KP=12,.KI=0,.KD=0,.KF=200,
				//.KP=12.7,.KI=0,.KD=0,
				//.KP=12.5,.KI=0,.KD=0,
				
				.max=angle_pid_max,.min=0,
				.i_max=3000,
				.f_max=3000,
				.dead_max=0.07,.dead_min=0}
		},
/******************************************yaw*******************************************************/											
										.yaw={
			.w={//角速度
				.mode={.out_put=1,.ffc=0,.i_trapezoid=0,.i_change=0},	
				.KP=294/**360/8192*/,.KI=0.9/**360/8192*/,.KD=-35,.KF=0,
				.i_change_a=0.1*pid_i_max,.i_change_b=0.8*pid_i_max,
				//.KP=270,.KI=0.7,.KD=20,
				//.KP=100,.KI=0.6,.KD=0,
				.max=DJI_gm6020_new_i_output_max,
				.i_max=5000,
				.f_max=2000,
				.dead_max=0,.dead_min=0	},
			.angle={//角度
				.mode={.out_put=1,.ffc=0,.i_trapezoid=0,.i_change=0},		
				.KP=17,.KI=0,.KD=0,.KF=0000,//2000
				//.KP=24,.KI=0,.KD=0,4
				//.KP=10,.KI=0,.KD=0,
				.max=angle_pid_max,
				.i_max=120,
				.f_max=3000,
				.dead_max=0.05,.dead_min=0	
			}
		}
	},
/*************************************************发射机构************************************************/											
												.shoot={
/****************************************************摩擦轮*********************************************/											
												.fire={
			.l.v={//左转速
				.mode={.out_put=1,.ffc=0,.i_trapezoid=0,.i_change=0},	
				.KP=16,.KI=0.12,.KD=0,.KF=0,
				.max=DJI_c620_output_max,
				.i_max=pid_i_max,
				.f_max=0,
				.dead_max=0,.dead_min=0	
			},
			.r.v={//右转速
				.mode={.out_put=1,.ffc=0,.i_trapezoid=0,.i_change=0},	
				.KP=15,.KI=0.09,.KD=0,.KF=0,
				.max=DJI_c620_output_max,
				.i_max=pid_i_max,
				.f_max=0,
				.dead_max=0,.dead_min=0	
			}
		},
/*************************************************拨盘*************************************************/													
												.knob={
			.w={//角速度
				.mode={.out_put=1,.ffc=0,.i_trapezoid=0,.i_change=0},	
				.KP=120 ,.KI=0,.KD=-300,.KF=0,
				.max=DJI_c610_output_max,
				.i_max=pid_i_max,
				.f_max=0,
				.dead_max=0,.dead_min=0	
			},
			.w_once={//单发角速度
				.mode={.out_put=1,.ffc=0,.i_trapezoid=0,.i_change=1},	
				.KP=15,.KI=1.2,.KD=10,.KF=20,
				.max=DJI_c610_output_max,
				.i_max=DJI_c610_output_max,
				.f_max=1000,
				.dead_max=0,.dead_min=0	
			},
			.angle={//单发角度
				.mode={.out_put=1,.ffc=0,.i_trapezoid=0,.i_change=1},	
				.KP=1,.KI=0,.KD=0,.KF=0,
				.max=DJI_c610_output_max,
				.i_max=DJI_c610_output_max,
				.f_max=0,
				.dead_max=0,.dead_min=0	
			}
		}
	}
};
head_pid_t* head_pid=&Head_PID;//云台的pid

#endif
/**
 * @brief 过零函数（走最短路径调整当前值，避免绕大圈）
 * @details 用于处理周期性数值（如角度）的差值计算，通过调整当前值，使当前值与目标值的差值在周期范围内取最短路径。
 * @param measure 当前测量值（如角度、位置等周期性数值）
 * @param target 目标值（需要调整到的目标位置）
 * @param T 数值的周期（如角度周期为360°时T=360）
 * @param k 过零比例系数（建议取0.5，表示当差值超过周期的一半时触发过零调整）
 * @return float 调整后的当前值（使得到目标值的路径最短）
 * @example over_zero(10+360,20,360,0.5) return 10
 * @example over_zero(10+360+180,20,360,0.5) return 10+180-180
 */
float over_zero(float measure /*现在数值*/,float target /*目标数值*/,int T,float k)//一般k取0.5
{
	if(target-measure>=(T*k))measure+=T;
	if(target-measure<=(-T*k))measure-=T;
	return measure;
}
//void PID_Init(pid_t* pid,float kp,float ki,float kd,float max,float min,float i_max,float dead_min,float dead_max )
//{//pid,kp,ki,kd,最大值,最小值,积分最大值,死区min，死区max
//	pid->KP=kp;
//	pid->KI=ki;
//	pid->KD=kd;
//	pid->target=/*理论值*/pid->err=/*上次误差*/pid->err_last=/*本次误差*/pid->integral=/*积分*/pid->output=/*P输出*/pid->output=/*I输出*/pid->output=/*D输出*/pid->output=0;/*输出*/	
//	pid->max=max;
//	pid->min=min;
//	pid->i_max=i_max;
//	pid->dead_min=dead_min;
//	pid->dead_max=dead_max;
//}

/**
 * @brief PID控制器核心计算函数
 * @details 实现PID控制算法，包含比例(P)、积分(I)、微分(D)项计算，支持前馈控制(FFC)、积分分离、梯形积分等扩展功能
 * 			计算公式 pid->out=kf*(tar[now]-tar[last])+kp*(err[now]-err[last])+ki*pid->integra+kd*(pid->err[last]-pid->err[now])
 * @note 其中k0是偏置项，提供外部补偿接口，默认为0
 * @param[in,out] pid PID控制器结构体指针，存储控制参数及中间状态
 * @param[in] measure 当前测量值（如电机转速、舵机角度等）
 * @param[in] target 目标设定值（期望达到的控制量）
 * @return float pid的输出值
 */ 
float PID_Calculate(pid_t* pid,float measure /*测量值*/,float target /*理论值*/){
	if(!pid)//检查是否是空指针
		return 0;
	/*--------储存上一刻的值----------*/
	pid->target[last]=pid->target[now];
	pid->measure[last]=pid->measure[now];
	pid->err[last]=pid->err[now];
	/*--------获取当前数值----------*/
	pid->target[now]=target;
	pid->measure[now]=measure;
	pid->err[now]=QWF_Dead(pid->target[now]-pid->measure[now],pid->dead_max,-pid->dead_max);//误差大小(含死区),
	/*------------计算-------------*/
	if(pid->mode.out_put){
		/*------------------------前馈F-----------------------*/
		if(pid->mode.ffc){
			pid->f_out=pid->KF*(pid->target[now]-pid->target[last]);
			if(QWF_Esp(pid->f_out,0)>=pid->f_max)
				pid->f_out=QWF_Signed(pid->f_out,0)*pid->f_max;
		}
		else
			pid->f_out=0;
		/*------------------------比例P-----------------------*/
		pid->p_out=pid->KP*pid->err[now];
		/*------------------------积分I-----------------------*/
		if(pid->mode.i_trapezoid)//梯形积分
			pid->integral+=(pid->err[now]+pid->err[last])/2;			
		else
			pid->integral+=pid->err[now];
		pid->integral=QWF_Limiter(pid->integral,pid->i_max*1.2f,-pid->i_max*1.2f);//积分限幅
		pid->i_out=pid->KI*pid->integral;
		if(pid->mode.i_change){
			if(pid->err[now]*pid->i_out>0){//先积分分离
				if(QWF_Esp(pid->err[now],0)<=pid->i_change_a)
					pid->i_out=pid->KI*pid->integral;
				else if(QWF_Esp(pid->err[now],0)<=(pid->i_change_a+pid->i_change_b))
					pid->i_out*=(pid->i_change_a-QWF_Esp(pid->err[now],0)+pid->i_change_b)/pid->i_change_a;
				else
				pid->i_out=0;
			}
		}
		/*------------------------微分D-----------------------*/
		pid->d_out=pid->KD*(pid->err[last]-pid->err[now]);	
		/*------------------------输出-----------------------*/
		pid->output=pid->f_out+pid->p_out+pid->i_out+pid->d_out+pid->K0;/*输出*/
		if(QWF_Esp(pid->output,0)>=pid->max)//输出限幅
			pid->output=QWF_Signed(pid->output,0)*pid->max;
		else if(QWF_Esp(pid->output,0)<=pid->min)
			pid->output=QWF_Signed(pid->output,0)*pid->min;
	}
	else
		pid->output=0;

	return pid->output;
}
//增量式

//ffc

//P_Mode

//I_Mode

//D_Mode

//Filter


